aa = FKdata.YawAngle+constrain(0.5*(DYTdata.Yaw_FrameAngle+DYTdata.Off_target_X)+10,30, -30);
% plot(time,FKdata.YawAngle+constrain(0.5*(DYTdata.Yaw_FrameAngle+DYTdata.Off_target_X)+10,30, -30),'k');%离线计算姿态指令
plot(time, aa,'k');
grid on;hold on;
plot(time,HK2FKdata.detaYAW,'r');%存储的姿态指令

grid on;hold on;
plot(time, b);grid on;hold on;
plot(time, 10 +constrain(b, 30, -30));grid on;hold on;



grid on;hold on;
plot(time-134, FKdata.AltituToG,'k');grid on;hold on;
legend('FK高度','HK高度');



grid on;hold on;
plot(time-134, HK2FKdata.detaYAW-30,'k');grid on;hold on;
legend('FK期望偏航角','FK偏航角','HK偏航指令');

grid on;hold on;
plot(time-134, FKdata.YawAngle,'k');grid on;hold on;
legend('FK期望偏航角','FK偏航角','HK记录偏航角');


grid on;hold on;
plot(time-134, HK2FKdata.detaPITCH,'k');grid on;hold on;
legend('FK期望俯仰角','FK俯仰角','HK俯仰指令');



grid on;hold on;
plot(time-134, FKdata.PitchAngle,'k');grid on;hold on;
legend('FK期望俯仰角','FK俯仰角','HK记录俯仰角');


grid on;hold on;
plot(time-134, FKdata.Air_speed,'k');grid on;hold on;
legend('FK空速','HK记录空速');


% 地速
% X轴方向速度
FKdata.X_Speed = (dataty(:,33)*256+dataty(:,32));
FKdata.X_Speed = double(typecast(uint16(FKdata.X_Speed),'int16'))*0.1;
% Y轴方向速度
FKdata.Y_Speed = (dataty(:,35)*256+dataty(:,34));
FKdata.Y_Speed = double(typecast(uint16(FKdata.Y_Speed),'int16'))*0.1;
% Z轴方向速度
FKdata.Z_Speed = (dataty(:,37)*256+dataty(:,36));
FKdata.Z_Speed = double(typecast(uint16(FKdata.Z_Speed),'int16'))*0.1;
% 惯性系速度
FKdata.Inertial_Speed = sqrt(FKdata.X_Speed.^2 + FKdata.Y_Speed.^2 );
grid on;hold on;
plot(time-134, FKdata.Inertial_Speed,'k');grid on;hold on;
legend('FK空速','FK地速','HK解算水平速度');

%%%%%%分析俯仰指令
plot(time, FKdata.PitchAngle + 0.8 * (DYTdata.Pitch_FrameAngle + DYTdata.Off_target_Y));grid on;hold on;
plot(time, HK2FKdata.detaPITCH);grid on;
%%%%%%%%%%%%%%

plot(time,DYTdata.Laser_range);grid on;


should_show(1, 1, 1,1)






